#ifndef __ROBOT_H_
#define __ROBOT_H_

#include <string>
#include <vector>
#include <iostream>
#include <fstream>
#include "Transform.h"

class EnvironmentBase;
class ModelInstance;

enum RobotType {RT_BASE, RT_RIGID_3T, RT_RIGID_3T3R, RT_ARTICULATED};
enum JointType {JT_FIXED, JT_REVOLUTE, JT_PRISMATIC, JT_CONTINUOUS, JT_UNKNOWN};

class RobotBase
{
public:
	RobotBase(EnvironmentBase* penv_);
	
	virtual ~RobotBase();
	
	EnvironmentBase* get_env() const;
	
	void set_bbox_state(float lower[], float upper[]);
	
	void set_dist_state_coeffs(float coeffs[]);
	
	void set_dist_state_coeffs(float coeff);
	
	virtual void load(std::string const& rob_file_name) = 0;
	
	int dim_state;
	int joint_num; // for articulated body (ignore it for rigid body)
	float* state_lower_bounds;
	float* state_upper_bounds;
	float* dist_state_coeffs; // coefficients for weighting when computing the distance
	EnvironmentBase* penv;
	RobotType robot_type;
};


class RigidBodyRobot3T : public RobotBase
{
public:
	RigidBodyRobot3T(EnvironmentBase* penv_);
	
	virtual void load(std::string const& rob_file_name);
	
	ModelInstance* model;
};

class RigidBodyRobot3T3R : public RobotBase
{
public:
	RigidBodyRobot3T3R(EnvironmentBase* penv_);
	
	virtual void load(std::string const& rob_file_name);
	
	ModelInstance* model;
};

class ArticulatedRobot : public RobotBase
{
public:
	// Joint is 1 DOF
	struct Joint
	{
		Transform T0;
		Transform Tq;
		
		float axis[3];
		JointType joint_type;
		
		short parent_idx; //idx of parent joint
		
		short param_idx;
		
		std::string name;
		
		float j_min, j_max;

		bool locked;
		
		ModelInstance* model;
	};
	
	class RobFileScanner
	{
	public:
		RobFileScanner(const char* fname);
		~RobFileScanner()
		{
			delete f;
		};
		
		bool get_token(std::string& s, const char* expected = 0, bool exit_on_eof = true);
		void get_string_token(std::string& s);
	private:
		bool read_string(std::string& s, const char* expected = 0);
		std::ifstream* f;
		const char* fname;
	};
	
	struct NoSelfCollisionPair
	{
		NoSelfCollisionPair(int i, int j): joint1(i), joint2(j) {};
		int joint1, joint2;
	};
	
	ArticulatedRobot(EnvironmentBase* penv_) : RobotBase(penv_)
	{
		joints = NULL;
		robot_type = RT_ARTICULATED;
	}
	
	
	ArticulatedRobot(EnvironmentBase* penv_, int dim) : RobotBase(penv_)
	{
		dim_state = dim;
		state_lower_bounds = new float[dim_state];
		state_upper_bounds = new float[dim_state];
		dist_state_coeffs = new float[dim_state];
		joints = new Joint[dim_state];
	}
	
	virtual void load(std::string const& rob_file_name);
	
	void load_rob(std::string const& rob_file_name);
	void load_urdf(std::string const& rob_file_name);
	void save_rob(std::string const& rob_file_name);
	
	~ArticulatedRobot()
	{
		delete [] state_lower_bounds;
		state_lower_bounds = NULL;
		delete [] state_upper_bounds;
		state_upper_bounds = NULL;
		delete [] dist_state_coeffs;
		dist_state_coeffs = NULL;
		delete [] joints;
		joints = NULL;
	}
	
	Joint* joints;
	
	std::vector<NoSelfCollisionPair> noSelfcollision_info;
};


#endif